/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

// #define DEBUG_EDGEFILTER

using System;
using Jitter2.Collision.Shapes;
using Jitter2.LinearMath;

using SoftFloat;

namespace Jitter2.Collision
{
	
	/// <summary>
	/// Level geometry is often represented by multiple instances of <see cref="Collision.Shapes.TriangleShape"/>
	/// added to a <see cref="Dynamics.RigidBody"/>. Other rigid bodies sliding over these triangles
	/// might encounter "internal edges", resulting in jitter. The <see cref="TriangleEdgeCollisionFilter"/>
	/// implements the <see cref="INarrowPhaseFilter"/> to help filter out these internal edges.
	/// </summary>
	public class TriangleEdgeCollisionFilter : INarrowPhaseFilter
	{
	    /// <summary>
	    /// A tweakable parameter. Collision points that are closer than this value to a triangle edge
	    /// are considered as edge collisions and might be modified or discarded entirely.
	    /// </summary>
	    public sfloat EdgeThreshold { get; set; } = (sfloat)0.05f;
	
	    private sfloat cosAT = (sfloat)0.99f;
	
	    /// <summary>
	    /// A tweakable parameter.
	    /// </summary>
	    public sfloat ProjectionThreshold { get; set; } = (sfloat)0.5f;
	
	    /// <summary>
	    /// A tweakable parameter that defines the threshold to determine when two normals
	    /// are considered identical.
	    /// </summary>
	    public JAngle AngleThreshold
	    {
	        get => JAngle.FromRadiant(libm.Acos(cosAT));
	        set => cosAT = libm.Cos(value.Radiant);
	    }
	
	    /// <inheritdoc />
	    public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
	        ref NVector3 pointA, ref NVector3 pointB, ref NVector3 normal, ref sfloat penetration)
	    {
	        TriangleShape? ts1 = shapeA as TriangleShape;
	        TriangleShape? ts2 = shapeB as TriangleShape;
	
	        bool c1 = ts1 != null;
	        bool c2 = ts2 != null;
	
	        // both shapes are triangles or both of them are not -> return
	        if (c1 == c2) return true;
	
	        TriangleShape tshape;
	        NVector3 collP;
	
	        if (c1)
	        {
	            tshape = ts1!;
	            collP = pointA;
	        }
	        else
	        {
	            tshape = ts2!;
	            collP = pointB;
	        }
	
	        ref var triangle = ref tshape.Mesh.Indices[tshape.Index];
	
	        NVector3 tnormal = triangle.Normal;
	        tnormal = NVector3.Transform(tnormal, tshape.RigidBody!.Data.Orientation);
	
	        if (c2) tnormal.Negate();
	
	        // Make triangles penetrable from one side
	        if (NVector3.Dot(normal, tnormal) < -cosAT) return false;
	
	        tshape.GetWorldVertices(out NVector3 a, out NVector3 b, out NVector3 c);
	
	        NVector3 n, pma;
	        sfloat d0, d1, d2;
	
	        // TODO: this can be optimized
	        n = b - a;
	        pma = collP - a;
	        d0 = (pma - NVector3.Dot(pma, n) * n * ((sfloat)1.0f / n.LengthSquared())).LengthSquared();
	
	        n = c - a;
	        pma = collP - a;
	        d1 = (pma - NVector3.Dot(pma, n) * n * ((sfloat)1.0f / n.LengthSquared())).LengthSquared();
	
	        n = c - b;
	        pma = collP - b;
	        d2 = (pma - NVector3.Dot(pma, n) * n * ((sfloat)1.0f / n.LengthSquared())).LengthSquared();
	
	        if (libm.Min(libm.Min(d0, d1), d2) > EdgeThreshold) return true;
	
	        NVector3 nnormal;
	
	        if (d0 < d1 && d0 < d2)
	        {
	            if (triangle.NeighborC == -1) return true;
	            nnormal = tshape.Mesh.Indices[triangle.NeighborC].Normal;
	        }
	        else if (d1 <= d0 && d1 < d2)
	        {
	            if (triangle.NeighborB == -1) return true;
	            nnormal = tshape.Mesh.Indices[triangle.NeighborB].Normal;
	        }
	        else
	        {
	            if (triangle.NeighborA == -1) return true;
	            nnormal = tshape.Mesh.Indices[triangle.NeighborA].Normal;
	        }
	
	        nnormal = NVector3.Transform(nnormal, tshape.RigidBody.Data.Orientation);
	
	        if (c2)
	        {
	            nnormal.Negate();
	        }
	
	        // now the fun part
	        //
	        // we have a collision close to an edge, with
	        //
	        // tnormal -> the triangle normal where collision occurred
	        // nnormal -> the normal of neighbouring triangle
	        // normal  -> the collision normal
	        if (NVector3.Dot(tnormal, nnormal) > cosAT)
	        {
	            // tnormal and nnormal are the same
	            // --------------------------------
	            sfloat f5 = NVector3.Dot(normal, nnormal);
	            sfloat f6 = NVector3.Dot(normal, tnormal);
	
	            if (f5 > f6)
	            {
	#if DEBUG_EDGEFILTER
	                Console.WriteLine($"case #1: adjusting; normal {normal} -> {nnormal}");
	#endif
	                if (f5 < ProjectionThreshold)
	                {
	                    return false;
	                }
	
	                penetration *= f5;
	                normal = nnormal;
	            }
	            else
	            {
	#if DEBUG_EDGEFILTER
	                Console.WriteLine($"case #1: adjusting; normal {normal} -> {tnormal}");
	#endif
	                if (f6 < ProjectionThreshold)
	                {
	                    return false;
	                }
	
	                penetration *= f6;
	                normal = tnormal;
	            }
	
	            return true;
	        }
	        // nnormal and tnormal are different
	        // ----------------------------------
	
	        // 1st step, project the normal onto the plane given by tnormal and nnormal
	        NVector3 cross = nnormal % tnormal;
	        NVector3 proj = normal - cross * normal * cross;
	
	        if (proj.LengthSquared() < ProjectionThreshold * ProjectionThreshold)
	        {
	#if DEBUG_EDGEFILTER
	            Console.WriteLine($"case #3: discarding");
	
	#endif
	            // can not project onto the plane, discard
	            return false;
	        }
	
	        // 2nd step, determine if "proj" is between nnormal and tnormal
	        //
	        //    /    nnormal
	        //   /
	        //  /
	        //  -----  proj
	        // \
	        //  \
	        //   \     tnormal
	        sfloat f1 = proj % nnormal * cross;
	        sfloat f2 = proj % tnormal * cross;
	
	        bool between = f1 * f2 <= (sfloat)0.0f;
	
	        if (!between)
	        {
	            // not in-between, snap normal
	            sfloat f3 = NVector3.Dot(normal, nnormal);
	            sfloat f4 = NVector3.Dot(normal, tnormal);
	
	            if (f3 > f4)
	            {
	#if DEBUG_EDGEFILTER
	                Console.WriteLine($"case #2: adjusting; normal {normal} -> {nnormal}");
	
	#endif
	                penetration *= f3;
	                normal = nnormal;
	            }
	            else
	            {
	#if DEBUG_EDGEFILTER
	                Console.WriteLine($"case #2: adjusting; normal {normal} -> {tnormal}");
	#endif
	                penetration *= f4;
	                normal = tnormal;
	            }
	        }
	
	        return true;
	    }
	}
}
